#include "ano_sensor.h"
#include "statistics.h"

extern SensorInfo_TypeDef sensor_info;


#define MAX_ANO_SENSOR_BUF_SIZE 128

 uint8_t ano_sensor_recv_data = 0;
 uint16_t ano_sensor_recv_len = 0;
 uint8_t ano_sensor_recv_buf[MAX_ANO_SENSOR_BUF_SIZE] = {0};
 uint16_t ano_sensor_expect_pack_len = 0;
 ano_sensor_optical_flow_data_t ano_sensor_optical_flow;
 ano_sensor_distance_data_t ano_sensor_distance;
 ano_sensor_imu_data_t  ano_sensor_imu;
 ano_sensor_quaternion_data_t ano_sensor_quaternion;

static void ano_get_check(uint8_t *buf,uint16_t len, ano_sensor_end_t *ender)
{
    uint8_t sumcheck = 0;
    uint8_t addcheck = 0;
		uint16_t i=0;
    for( i=0; i < len; i++)
    {
        sumcheck += buf[i];
        addcheck += sumcheck;
    }
    ender->sum_check = sumcheck;
    ender->add_check = addcheck;
}


void ano_sensor_init(void)
{
  ano_sensor_start_recv(&ano_sensor_recv_data);
	statistics_register(STATISTICS_TOF_NAME,1000);
	statistics_register(STATISTICS_OPTICAL_FLOW_NAME,1000);
}


void ano_sensor_recv_parse(void)
{
    if( ano_sensor_recv_len )
    {
        ano_sensor_recv_buf[ano_sensor_recv_len] = ano_sensor_recv_data;
        ano_sensor_recv_len++;          
        if( ano_sensor_recv_len == sizeof(ano_sensor_header_t) )
        {
            ano_sensor_header_t header;
            memcpy((uint8_t*)&header,ano_sensor_recv_buf,sizeof(ano_sensor_header_t));            
            ano_sensor_expect_pack_len = header.length;
            /*get the expect length*/
        }
        else if( ano_sensor_recv_len == (ano_sensor_expect_pack_len + sizeof(ano_sensor_header_t) + sizeof(ano_sensor_end_t)) )
        {
            ano_sensor_end_t pack_ender,cal_ender;
            memcpy((uint8_t*)&pack_ender,ano_sensor_recv_buf+(ano_sensor_expect_pack_len + sizeof(ano_sensor_header_t)),sizeof(ano_sensor_end_t));
            /* get the check number in pack */

            ano_get_check(ano_sensor_recv_buf,ano_sensor_expect_pack_len + sizeof(ano_sensor_header_t),&cal_ender);
            /* get the check */

            if( pack_ender.sum_check == cal_ender.sum_check && pack_ender.add_check == cal_ender.add_check )
            {
                /* check ok */
                ano_sensor_header_t header;
                memcpy((uint8_t*)&header,ano_sensor_recv_buf,sizeof(ano_sensor_header_t));   

                union 
                {
                    int16_t s16_;
                    uint8_t u8_[2];
                }tmp;
                switch( header.func_code)
                {
                    case 0x51: /* OF DATA*/
                        if( header.length == 5) //mode = 0
                        {
                            ano_sensor_optical_flow.mode = ano_sensor_recv_buf[sizeof(ano_sensor_header_t)];
                            ano_sensor_optical_flow.state = ano_sensor_recv_buf[sizeof(ano_sensor_header_t) + 1];                            
                            ano_sensor_optical_flow.of_raw_x = (int8_t)ano_sensor_recv_buf[sizeof(ano_sensor_header_t) + 2];
                            ano_sensor_optical_flow.of_raw_y = (int8_t)ano_sensor_recv_buf[sizeof(ano_sensor_header_t) + 3];
                            ano_sensor_optical_flow.quality = ano_sensor_recv_buf[sizeof(ano_sensor_header_t) + 4];
													
//														statistics_update(STATISTICS_OPTICAL_FLOW_NAME);
                        }
                        else if( header.length == 7) // mode = 1
                        {
                            ano_sensor_optical_flow.mode = ano_sensor_recv_buf[sizeof(ano_sensor_header_t)];
                            ano_sensor_optical_flow.state = ano_sensor_recv_buf[sizeof(ano_sensor_header_t) + 1]; 
                            memcpy(tmp.u8_,ano_sensor_recv_buf+sizeof(ano_sensor_header_t)+2,2);
                            ano_sensor_optical_flow.of_decouple_x = tmp.s16_;
                            memcpy(tmp.u8_,ano_sensor_recv_buf+sizeof(ano_sensor_header_t)+4,2);
                            ano_sensor_optical_flow.of_decouple_y = tmp.s16_;
                            ano_sensor_optical_flow.quality = ano_sensor_recv_buf[sizeof(ano_sensor_header_t) + 6];
													
//														statistics_update(STATISTICS_OPTICAL_FLOW_NAME);
                        }
                        else if( header.length == 15) // mode = 2
                        {
                            ano_sensor_optical_flow.mode = ano_sensor_recv_buf[sizeof(ano_sensor_header_t)];
                            ano_sensor_optical_flow.state = ano_sensor_recv_buf[sizeof(ano_sensor_header_t) + 1]; 
                            memcpy(tmp.u8_,ano_sensor_recv_buf+sizeof(ano_sensor_header_t)+2,2);
                            ano_sensor_optical_flow.of_fusion_x = tmp.s16_;
                            memcpy(tmp.u8_,ano_sensor_recv_buf+sizeof(ano_sensor_header_t)+4,2);
                            ano_sensor_optical_flow.of_fusion_y = tmp.s16_;
                            memcpy(tmp.u8_,ano_sensor_recv_buf+sizeof(ano_sensor_header_t)+6,2);
                            ano_sensor_optical_flow.of_fix_x = tmp.s16_;
                            memcpy(tmp.u8_,ano_sensor_recv_buf+sizeof(ano_sensor_header_t)+8,2);
                            ano_sensor_optical_flow.of_fix_y = tmp.s16_;
                            memcpy(tmp.u8_,ano_sensor_recv_buf+sizeof(ano_sensor_header_t)+10,2);
                            ano_sensor_optical_flow.of_integral_x = tmp.s16_;
                            memcpy(tmp.u8_,ano_sensor_recv_buf+sizeof(ano_sensor_header_t)+12,2);
                            ano_sensor_optical_flow.of_integral_y = tmp.s16_;
                            ano_sensor_optical_flow.quality = ano_sensor_recv_buf[sizeof(ano_sensor_header_t) + 14]; 
													
														statistics_update(STATISTICS_OPTICAL_FLOW_NAME);
                        }
												else
												{
													ano_sensor_optical_flow.mode = 0xff;
													ano_sensor_optical_flow.state = 0;
													/*invalid sensor flow data*/
												}
												
												//update the optical flow data to sensor_info struct
												if( ano_sensor_optical_flow.state == 1 )
												{
													//data valid
													if( ano_sensor_optical_flow.mode == 2 )
													{
														sensor_info.raw_data.of_speed_x = ano_sensor_optical_flow.of_fusion_x * 1.f;
														sensor_info.raw_data.of_speed_y = ano_sensor_optical_flow.of_fusion_y * 1.f;
														//cm/s
													}
												}
												
                    break;
                    case 0x34:  /*distance*/
                        memcpy((uint8_t*)&ano_sensor_distance,ano_sensor_recv_buf+sizeof(ano_sensor_header_t),sizeof(ano_sensor_distance_data_t));
                        //get distance data
												if( ano_sensor_distance.distance != 0xFFFFFFFF )
												{
													//valid data
													sensor_info.raw_data.tof.distance = ano_sensor_distance.distance * 1.f;
													sensor_info.raw_data.tof.tick = HAL_GetTick();
													sensor_info.raw_data.tof.update_flag = 1;
												}
												statistics_update(STATISTICS_TOF_NAME);
                    break;
                    case 0x01:/*IMU*/
                        memcpy((uint8_t*)&ano_sensor_imu,ano_sensor_recv_buf+sizeof(ano_sensor_header_t),sizeof(ano_sensor_imu_data_t));
                    break;
                    case 0x04: /*quaternion*/
                        memcpy((uint8_t*)&ano_sensor_quaternion,ano_sensor_recv_buf+sizeof(ano_sensor_header_t),sizeof(ano_sensor_quaternion_data_t));
                    break;
                    default:
                    /*invalid data*/
                    break;
                }
            }
            ano_sensor_expect_pack_len = 0;
						ano_sensor_recv_len = 0;
            /*reset the length*/
        }
    }
    else
    {
        /* first recv */
        if( ano_sensor_recv_data == 0xAA )
        {
            ano_sensor_recv_buf[ano_sensor_recv_len] = ano_sensor_recv_data;
            ano_sensor_recv_len++;
        }
    }
    ano_sensor_start_recv(&ano_sensor_recv_data);
}



__WEAK void ano_sensor_start_recv(uint8_t *data)
{
    UNUSED(data);
}



